#include "main.h"

CNode::CNode* GetBestNode(CNode::CNode* EndNode, CNode::CNode* CurrentNode, std::vector<CNode::CNode*> NodeList)
{
	int MinIndex = 0;
	float MinScore = ComputeScore(EndNode, CurrentNode, NodeList[0]);

	for(unsigned int i = 1; i < NodeList.size(); i++)
	{
		if(NodeList[i] != CurrentNode)
		{
			float tmpScore = ComputeScore(EndNode, CurrentNode, NodeList[i]);
			if(tmpScore < MinScore)
			{
				MinScore = tmpScore;
				MinIndex = i;
			}
		}
	}

	return NodeList[MinIndex];
}

float ComputeScore(CNode::CNode* EndNode, CNode::CNode* CurrentNode, CNode::CNode* NextNode)
{
	float G = GetPathScore(CurrentNode->_X, CurrentNode->_Y, NextNode->_X, NextNode->_Y);
	float H = GetPathScore(NextNode->_X, NextNode->_Y, EndNode->_X, EndNode->_Y);

	return G + H;
}

float GetPathScore(int CurrentX, int CurrentY, int NextX, int NextY)
{
	return sqrt((float)((NextX - CurrentX) * (NextX - CurrentX)) + (float)((NextY - CurrentY) * (NextY - CurrentY)));
}

int IsInList(CNode::CNode* ptrValue, std::vector<CNode::CNode*> NodeList)
{
	for(unsigned int i = 0; i < NodeList.size(); i++)
		if(NodeList.at(i) == ptrValue)
			return i;

	return -1;
}

std::vector<CNode::CNode*> RemoveFromList(CNode::CNode* ptrValue, std::vector<CNode::CNode*> NodeList)
{
	std::vector<CNode::CNode*> tmpList;

	for(unsigned int i = 0; i < NodeList.size(); i++)
		if(NodeList.at(i) != ptrValue)
			tmpList.push_back(NodeList.at(i));

	return tmpList;
}

void PathFind(CGrid::CGrid Lab)
{
	std::vector<CNode::CNode*> OpenList;
	std::vector<CNode::CNode*> ClosedList;
	CNode::CNode* CurrentNode;

	CurrentNode = Lab.StartNode;
	OpenList.push_back(CurrentNode);

	while(CurrentNode != Lab.EndNode)
	{
		std::vector<CNode::CNode*> Adjacents = Lab.GetAdjacentNodes(CurrentNode->_X, CurrentNode->_Y);

		// On cherche les noeuds adjacents qui ne sont pas des murs et qui ne sont pas dans la liste fermée.
		for(unsigned int i = 0; i < Adjacents.size(); i++)
		{
			if(Adjacents[i]->_Type != WALL && IsInList(Adjacents[i], ClosedList) == -1)
			{
				// S'il n'est pas dans la liste fermée, on l'ajoute
				if(IsInList(Adjacents[i], OpenList) == -1)
				{
					Adjacents[i]->Parent = CurrentNode;
					OpenList.push_back(Adjacents.at(i));
				}
				// Sinon, on la met à jour
				else if(IsInList(Adjacents[i], OpenList) != -1)
				{
					int NodeIndex = IsInList(Adjacents[i], OpenList);
					OpenList[NodeIndex]->Parent = CurrentNode;
				}
			}
		}

		if(OpenList.size() == 0)
		{
			std::cout << "Pas de solution... Fin de l'algorithme !" << std::endl;
			break;
		}

		OpenList = RemoveFromList(CurrentNode, OpenList);
		ClosedList.push_back(CurrentNode);

		CurrentNode = GetBestNode(Lab.EndNode, CurrentNode, OpenList);
	}

	CNode::CNode* ParentNode = CurrentNode;

	while(ParentNode != NULL)
	{
		ParentNode->IsPath = 1;
		ParentNode = ParentNode->Parent;
	}

	std::cout << "Solution Trouvé !" << std::endl;
}

int main ( int argc, char** argv )
{
    SDLToolBox::SDLToolBox ToolBox;
    ToolBox.InitSDL(800, 600);

    SDL_Event Event;
    int Exit = 0;

    CGrid::CGrid Lab((char*)("/tmp/1.lab"), 800, 600);
    if(argc == 1) PathFind(Lab);

    while(!Exit)
    {
        SDL_PollEvent(&Event);

        switch(Event.type)
        {
                case SDL_QUIT:
                    Exit = 1;
                break;
        }

            ToolBox.Clear(255, 255, 255);
            Lab.DrawAllNodes(ToolBox.Screen);
            ToolBox.Render();
    }

    ToolBox.CleanUpSDL();
    return 0;
}
